/*
   svdc.c -- Singular value decomposition. Translated to 'C' from the
            original Algol code in "Handbook for Automatic Computation,
            vol. II, Linear Algebra", Springer-Verlag.
 
   (C) 2000, C. Bond. All rights reserved.
 
   This is almost an exact translation from the original, except that
   an iteration counter is added to prevent stalls. This corresponds
   to similar changes in other translations.
 
   Returns an error code = 0, if no errors and 'k' if a failure to
   converge at the 'kth' singular value.
   
   Taken from http://www.crbond.com/download/misc/svd.c and adapted for
   the use in Matlab
  
 */

#include "mex.h"
#include "matrix.h"
#include "math.h"

int svdc(int m, int n, double eps, const double *a, double *q, double *u, double *v) {
    
    /* variables */
	int i, j, k, ki, l, l1, mn, iter;
	double c, dv, f, g, h, s, x, y, z;
	double *e, *up1, *up2, *vp1, *vp2;

	e = (double *) calloc(n, sizeof(double));
    if (e == NULL)
        return -1;
    
    mn = m * n;

    /* copy a to u */   
	for (i = mn, up1 = u; i > 0; --i)
		*up1++ = *a++;
    
    /* Householder's reduction to bidiagonal form. */
	g = x = 0.0;
    
    /* loop over columns */
	for (i = 0; i < n; ++i) {
		e[i] = g;
		l = i + 1;
        
        /* init sum to 0 */
		s = 0.0;
        
        /* sum all elements incl. and below diagonal */
		for (j = i, up1 = &u[i + i * m]; j < m; ++j) {
            dv = *up1++;
			s += dv * dv;
        }
        
        /* anything at all */
		if (s == 0.0)
			g = 0.0;
        
        /* otherwise */
		else {
            
            /* replace diagonal element */
            up1 = &u[i * m];
			f = up1[i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			up1[i] = f - g;
            
            /* and compute over remaining elements */
			for (j = l; j < n; ++j) {
				s = 0.0;
				for (k = i, up1 = &u[i + i * m], up2 = &u[i + j * m]; k < m; ++k)
					s += (*up1++ * *up2++);
				f = s / h;
				for (k = i, up2 = &u[i]; k < m; ++k) {
					up2[j * m] += f * up2[i * m];
                    ++up2;
                }
			}
		}
        
		q[i] = g;
		s = 0.0;
        up1 = &u[i];
		for (j = l; j < n; ++j) {
            dv = up1[j * m];
			s += dv * dv;
        }
		if (s == 0.0)
			g = 0.0;
		else {
            up2 = &up1[l * m];
			f = *up2;
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			*up2 = f - g;
			for (j = l; j < n; ++j) 
				e[j] = up1[j * m] / h;
			for (j = l; j < m; ++j) {
				s = 0.0;
				for (k = l, up2 = &u[j], up1 = &u[i]; k < n; ++k) {
                    ki = k * m;
					s += up1[ki] * up2[ki];
                }
				for (k = l, up2 = &u[j]; k < n; ++k)
					up2[k * m] += s * e[k];
			}
		}
		y = fabs(q[i]) + fabs(e[i]);                         
		if (y > x)
			x = y;
	}
    
    /* accumulation of right-hand transformations */
    for (i = n - 1 ; i >= 0; --i) {
        if (g != 0.0) {
            up1 = &u[i];
            h = up1[(i+1) * m] * g;
            for (j = l, vp1 = &v[l + i * n]; j < n; ++j)
                *vp1++ = up1[j * m] / h;
            for (j = l; j < n; ++j) {
                s = 0.0;
                for (k = l, vp1 = &v[l + j * n]; k < n; ++k) 
                    s += up1[k * m] * *vp1++;
                for (k = l, vp2 = &v[l + j * n], vp1 = &v[l + i * n]; k < n; ++k)
                    *vp2++ += s * *vp1++;

            }
        }
        for (j = l, vp1 = &v[i], vp2 = &v[i * n]; j < n; ++j)
            vp1[j * n] = vp2[j] = 0.0;
        vp2[i] = 1.0;
        g = e[i];
        l = i;
    }

    /* accumulation of left-hand transformations */
    for (i = n, up1 = &u[n]; i < m; ++i) {
        for (j = n; j < m; ++j)
            up1[j * m] = 0.0;
        up1[i * m] = 1.0;
    }
    for (i = n-1; i >= 0; --i) {
        l = i + 1;
        g = q[i];
        for (j = l, up1 = &u[i]; j < m; ++j)
            up1[j * m] = 0.0;
        if (g != 0.0) {
            h = up1[i * m] * g;
            for (j = l; j < m; ++j) {
                s = 0.0;
                for (k = l, up1 = &u[k + i*m], up2 = &u[k + j*m]; k < m; ++k)
                    s += (*up1++ * *up2++);
                f = s / h;
                for (k = i, up1 = &u[k + i*m], up2 = &u[k + j*m]; k < m; ++k) 
                    *up2++ += (f * *up1++);
            }
            for (j = i, up1 = &u[i + i * m]; j < m; ++j) 
                *up1++ /= g;
        } else {
            for (j = i, up1 = &u[i + i * m]; j < m; ++j)
                *up1++ = 0.0;
        }
        u[i + i * m] += 1.0;
    }

    /* diagonalization of the bidiagonal form */
	eps *= x;
	for (k = n-1; k >= 0; --k) {
        
        /* init max iter counter */
        iter = 30;
        
test_f_splitting:
		for (l = k; l >= 0; --l) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l-1]) <= eps) break;
		}

    /* cancellation of e[l] if l > 0 */
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i = l; i <= k; ++i) {
			f = s * e[i];
			e[i] *= c;
			if (fabs(f) <= eps) goto test_f_convergence;
			g = q[i];
			h = q[i] = sqrt(f * f + g * g);
			c = g / h;
			s = -f / h;
            for (j = 0, up1 = &u[l1 * m], up2 = &u[i * m]; j < m; ++j) {
                y = *up1;
                z = *up2;
                *up1++ = y * c + z * s;
                *up2++ = -y * s + z * c;
            }
		}
        
test_f_convergence:
		z = q[k];
		if (l == k) goto convergence;

/* shift from bottom 2x2 minor */
		if (--iter < 0) {
            free(e);
			return k;
		}
		x = q[l];
		y = q[k-1];
		g = e[k-1];
		h = e[k];
		f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2 * h * y);
		g = sqrt(f * f + 1.0);
		f = ((x - z) * (x + z) + h * (y / ((f < 0) ? (f - g) : (f + g)) - h)) / x;
/* next QR transformation */
		c = s = 1.0;
		for (i = l+1; i <= k; ++i) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i-1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
            for (j = 0, vp1 = &v[(i-1) * n], vp2 = &v[i * n]; j < n; ++j) {
                x = *vp1;
                z = *vp2;
                *vp1++ = x * c + z * s;
                *vp2++ = -x * s + z * c;
            }
			q[i-1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = c * g + s * y;
			x = -s * g + c * y;
            for (j = 0, up1 = &u[(i-1) * m], up2 = &u[i * m]; j < m; ++j) {
                y = *up1;
                z = *up2;
                *up1++ = y * c + z * s;
                *up2++ = -y * s + z * c;
            }
		}
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
        
convergence:
		if (z < 0.0) {
/* q[k] is made non-negative */
			q[k] = - z;
            for (j = 0, vp1 = &v[k * n]; j < n; ++j)
                *vp1++ = -*vp1;
		}
	}
	
	free(e);
    return 0;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    const int *mn;
    int mnout[2], c, m, n, r;
    const double *a;
    double *u, *s, *vt, *at, *atw;
    char vstr[255];
    
    /* argument check */
    if (nrhs != 1 || nlhs != 3)
		mexErrMsgTxt("Bad number of input/output arguments.");
    if ((mxGetNumberOfDimensions(*prhs) != 2) ||
        (mxGetNumberOfElements(*prhs) == 0) ||
        (mxGetClassID(*prhs) != mxDOUBLE_CLASS))
        mexErrMsgTxt("Invalid input argument.");
    mn = mxGetDimensions(*prhs);
    a = (const double*) mxGetData(*prhs);
    
    /* create output matrices */
    *mnout = m = *mn++;
    mnout[1] = m;
    *plhs = mxCreateNumericArray(2, mnout, mxDOUBLE_CLASS, mxREAL);
    s = (double *) mxGetData(*plhs);
    mnout[1] = n = *mn;
    plhs[1] = mxCreateNumericArray(2, mnout, mxDOUBLE_CLASS, mxREAL);
    u = (double *) mxGetData(plhs[1]);
    *mnout = n;
    plhs[2] = mxCreateNumericArray(2, mnout, mxDOUBLE_CLASS, mxREAL);
    vt = (double *) mxGetData(plhs[2]);
    /* if no more cols than rows */
    if (m >= n) {
        
        /* make call */
        svdc(m, n, 2.2e-16, a, u, s, vt);
        
        /* put elements in diagonal */
        s = ++u;
        for (c = 1; c < m; ++c) {
            s[c*m] = *u;
            *u++ = 0.0;
            ++s;
        }
        
    /* otherwise */
    } else {
        
        /* transpose a first */
        at = calloc(m * n, sizeof(double));
        if (at == NULL)
            mexErrMsgTxt("Error allocating space for matrix transpose.");
        for (c = 0; c < n; ++c) {
            atw = &at[c];
            for (r = m-1; r >= 0; --r) {
                *atw = *a++;
                atw = &atw[n];
            }
        }
        
        svdc(n, m, 1.0e-38, at, u, vt, s);
        free(at);
        
        /* put elements in diagonal */
        s = ++u;
        for (c = 1; c < m; ++c) {
            s[c*m] = *u;
            *u++ = 0.0;
            ++s;
        }
    }
}
